/*
 * usb.cpp
 *
 *  Created on: 11.03.2012
 *      Author: g.kruglov
 */

#include "usb.h"
#include "kl_lib.h"
#include "led.h"
#include "descriptors.h"

// To save UartSettings
#include "periph.h"

// ======================== Prototypes and variables ===========================
// Control Endpoint's additional objects
enum ControlStages_t {csWaitSetup, csWaitDataIn, csWaitDataOut, csWaitStatusIn, csWaitStatusOut} ControlStage;
EpxState_t Ep0TxState, Ep0RxState;
UsbRequest_t UsbRequest;
extern LedBlink_t RxLed, TxLed;

void In0Handler(void);
void Setup0Handler(void);
void Out0Handler(void);

// ============================== Implementation ===============================
void Usb_t::Init() {
    IsConfigured = false;
    klPinSetup(GPIOA, 15, pmOutPushPull, ps50MHz);
    IDisconnect();
    // ==== USB clock ====
    RCC_USBCLKConfig(RCC_USBCLKSource_PLLCLK_Div1);     // Select USBCLK source: SysClk = 48 => div 1
    RCC_APB1PeriphClockCmd(RCC_APB1Periph_USB, ENABLE); // Enable the USB clock
    // ==== Interrupt ====
    NVIC_InitTypeDef NVIC_InitStructure;
//    NVIC_PriorityGroupConfig(NVIC_PriorityGroup_1);      // FIXME
    NVIC_InitStructure.NVIC_IRQChannel = USB_LP_CAN1_RX0_IRQn;
    NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1;
    NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
    NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
    NVIC_Init(&NVIC_InitStructure);

//    IConnect();         // Connect pullup resistor
    *CNTR = CNTR_FRES;      // } Force RESET
    *CNTR = 0;              // } pulse reset
    *ISTR = 0;              // Clear pending interrupts
    *CNTR = (CNTR_CTRM | CNTR_RESETM);  // Setup IRQ mask: correct transfer & reset
}

void Usb_t::Send(uint8_t EpNum, uint8_t *PBuf, uint32_t ACount) {
    Ep[EpNum].Buf = PBuf;
    Ep[EpNum].Count = ACount;
    Ep[EpNum].WriteData();
    Ep[EpNum].SetTxState(epxValid);
}

void Usb_t::IRQHandler() {
    uint16_t IRQFlags = (uint16_t) *ISTR;
    if (IRQFlags & ISTR_CTR)   CTR_Handler();
    if (IRQFlags & ISTR_RESET) RST_Handler();
}

void Usb_t::Reset() {
    RST_Handler();
}
void Usb_t::HandlePkt() {
    CTR_Handler();
}

void Usb_t::RST_Handler() {
    *ISTR = CLR_RESET;  // Clear RST IRQ flag
//    CmdUnit.Printf("RST\r");
    // ==== Perform usb initialization ====
    *BTABLE = 0;    // Buffer allocation table is stored at zero address of USB RAM
    ControlStage = csWaitSetup;
    // Configure endpoints
    // Control endpoint
    Ep[0].Indx = 0;
    Ep[0].SetType   (EP_CONTROL);       // Control endpoint
    Ep[0].SetTxState(epxStall);         // Do not transmit
    Ep[0].SetTxAddr (ENDP0_TXADDR);     // Set address of TX Buffer in buffer allocation table
    Ep[0].SetRxAddr (ENDP0_RXADDR);     // Set address of RX Buffer in buffer allocation table
    Ep[0].ClearStatusOut();             // When STATUS_OUT is reset, OUT transactions can have any number of bytes, as required.
    Ep[0].SetRxCount(USB_MAX_PKT_SIZE);
    Ep[0].SetRxState(epxValid);         // This endpoint is enabled for reception

    // Endpoint 1
    Ep[1].Indx = 1;
    Ep[1].SetType   (EP_BULK);
    Ep[1].SetTxAddr (0xC0);
    Ep[1].SetTxState(epxNAK);       // Tx NAKed
    Ep[1].SetRxState(epxDisabled);  // Rx disabled

    // Endpoint 2
    Ep[2].Indx = 2;
    Ep[2].SetType   (EP_INTERRUPT);
    Ep[2].SetTxAddr (0x100);
    Ep[2].SetTxState(epxNAK);       // Tx NAKed
    Ep[2].SetRxState(epxDisabled);  // Rx disabled

    // Endpoint 3
    Ep[3].Indx = 3;
    Ep[3].SetType   (EP_BULK);
    Ep[3].SetRxAddr (0x110);
    Ep[3].SetRxCount(USB_MAX_PKT_SIZE);
    Ep[3].SetRxState(epxValid);     // Rx ready
    Ep[3].SetTxState(epxDisabled);  // Tx disabled

    // Set this device to respond on default address
    SetDeviceAddress(0);
}

void Usb_t::CTR_Handler() {
    //__disable_irq();
//    PrintfDisable();
//    CmdUnit.Printf("CTR ");
    uint16_t ISTRReg = (uint16_t) *ISTR;                // Point to interrupt ivents
    uint8_t EPIndx = (uint8_t)(ISTRReg & ISTR_EP_ID);   // Extract highest priority endpoint number
    uint16_t EPValue = Ep[EPIndx].GetReg();
    // Service control endpoint
    if (EPIndx == 0) {
        // Save TX & RX status
        Ep0TxState = Ep[0].GetTxState();
        Ep0RxState = Ep[0].GetRxState();
        Ep[0].SetTxState(epxNAK);
        Ep[0].SetRxState(epxNAK);
        // Handle event
        if ((ISTRReg & ISTR_DIR) == 0) {    // DIR == 0 => IN irq
            Ep[0].ClearCTR_TX();
//            CmdUnit.Printf("IN(%u): %A \r", Ep[0].GetTxCount(), Ep[0].Buf, Ep[0].GetTxCount());
            In0Handler();                   // Endpoint0 specific
     }
      else {  // SETUP or OUT               // DIR == 1
          // check if SETUP
          if (EPValue & EP_SETUP) {
              Ep[0].ClearCTR_RX();
//              CmdUnit.Printf("SETUP \r");
                Setup0Handler();            // Endpoint0 specific
          }
          else if (EPValue & EP_CTR_RX) {
                Ep[0].ClearCTR_RX();
//                CmdUnit.Printf("OUT(%u): %A \r", Ep[0].GetRxCount(), Ep[0].Buf, Ep[0].GetRxCount());
                Out0Handler();
          }
      }
      Ep[0].SetTxState(Ep0TxState);
      Ep[0].SetRxState(Ep0RxState);
    } // if EP0
    else {
        if(EPValue & EP_CTR_RX) {
//            CmdUnit.Printf("Rx\r");
            Ep[EPIndx].ClearCTR_RX();
            Ep[EPIndx].ReceivePkt();
            Ep[EPIndx].SetRxState(epxValid);    // Reenable endpoint
        }
        if(EPValue & EP_CTR_TX) {
            Ep[EPIndx].ClearCTR_TX();
            //if (Ep[EPIndx].Evt_Rx != 0) Ep[EPIndx].Evt_Rx();
        }
    }
    //__enable_irq();
//    PrintBuf();
//    CmdUnit.Printf("\r");
}

void Usb_t::SetDeviceAddress(uint8_t AAddress) {
//    CmdUnit.Printf("SetDeviceAddress: %u \r", AAddress);
    // Set address in every used endpoint
    for (uint8_t i=0; i<4; i++) Ep[i].SetAddress(i);
    // Set device address and enable function
    *DADDR = (AAddress | DADDR_EF);
}

// ================================ Endpoint ===================================
// Lot of stuff for not to toggle sensitive bits
void Endpoint_t::SetTxState(EpxState_t ATxState) {
    uint16_t RegValue = GetReg() & EPTX_DTOGMASK;
    switch (ATxState) {
        case epxDisabled: break;
        case epxStall: RegValue ^= 0x0010; break;
        case epxNAK:   RegValue ^= 0x0020; break;
        case epxValid: RegValue ^= 0x0030; break;
    }
    SetReg(RegValue | EP_CTR_RX | EP_CTR_TX);
}
void Endpoint_t::SetRxState(EpxState_t ARxState) {
    uint16_t RegValue = GetReg() & EPRX_DTOGMASK;
    switch (ARxState) {
        case epxDisabled: break;
        case epxStall: RegValue ^= 0x1000; break;
        case epxNAK:   RegValue ^= 0x2000; break;
        case epxValid: RegValue ^= 0x3000; break;
    }
    SetReg(RegValue | EP_CTR_RX | EP_CTR_TX);
}

EpxState_t Endpoint_t::GetTxState(void) {
    uint16_t RegValue = GetReg() & EPTX_STAT;
    switch (RegValue) {
        case 0x0010: return epxStall; break;
        case 0x0020: return epxNAK; break;
        case 0x0030: return epxValid; break;
        default: break;
    }
    return epxDisabled;
}
EpxState_t Endpoint_t::GetRxState(void) {
    uint16_t RegValue = GetReg() & EPRX_STAT;
    switch (RegValue) {
        case 0x1000: return epxStall; break;
        case 0x2000: return epxNAK; break;
        case 0x3000: return epxValid; break;
        default: break;
    }
    return epxDisabled;
}

void Endpoint_t::SetRxCount(uint16_t ACount) {
    uint32_t *pdwReg = pEPRxCount(Indx);
    uint16_t BlockCount;
    if(ACount > 62) {   // Write number of blocks of 32 bytes
        BlockCount = ACount >> 5;
        if((ACount & 0x1F) == 0) BlockCount--; // Even\odd check
        *pdwReg = (uint32_t)((BlockCount << 10) | 0x8000);    // Write number of blocks & set MSB to signal 32byte block
    }
    else {
        BlockCount = ACount >> 1;
        if((ACount & 0x1) != 0) BlockCount++;
        *pdwReg = (uint32_t)(BlockCount << 10);
    }
}

// Buffer works
void Endpoint_t::WriteBuffer(uint8_t *PBuffer, uint16_t ALength) {
//    klPrintf("WriteBuffer\r");
    uint32_t n = (ALength + 1) >> 1;   // n = (ALength + 1) / 2
    uint16_t *pPktBuf = (uint16_t*)(PMAAddr + (GetTxAddr() * 2)); // *2 for 32 bits addressing
    for (uint32_t i=0; i<n; i++) {
        *pPktBuf = ((uint16_t) *PBuffer) | (((uint16_t) *(PBuffer+1)) << 8);
        pPktBuf += 2;   // Inc by 4
        PBuffer += 2;   // Inc by 2
    }
}
void Endpoint_t::ReadBuffer(uint8_t *PBuffer, uint16_t ALength) {
    if ((PBuffer == 0) or (ALength == 0)) return;
    uint32_t n = (ALength + 1) >> 1;    // /2
    uint32_t *pPktBuf = (uint32_t*)(PMAAddr + (GetRxAddr() * 2));
    for (uint32_t i=0; i<n; i++) {
        *(uint16_t*)PBuffer = *pPktBuf;
        pPktBuf += 1;   // Inc by 4
        PBuffer += 2;   // Inc by 2
    }
}

uint16_t Endpoint_t::WriteData() {
    uint16_t SizeToWriteNow = (Count > USB_MAX_PKT_SIZE)? USB_MAX_PKT_SIZE : Count;
    if (SizeToWriteNow != 0) WriteBuffer(Buf, SizeToWriteNow);
    SetTxCount(SizeToWriteNow);
    Count -= SizeToWriteNow;
    Buf += SizeToWriteNow;
    //klPrintf("WriteData: %H\r", UsbTxData.Buf, SizeToWriteNow);
    return SizeToWriteNow;
}

void Endpoint_t::ReceivePkt() {
    if (Buf != 0) {
        Count = GetRxCount();
        ReadBuffer(Buf, Count);
        NewPktReceived = true;
    }
}

// ============= Endpoint0 handlers =============
void HandleCtrlRequest(void) {
//    CmdUnit.Printf("bmRequestType: %X; bRequest: %X; wValue: %X; wIndex: %X; wLength: %u\r", UsbRequest.bmRequestType, UsbRequest.bRequest, UsbRequest.wValue, UsbRequest.wIndex, UsbRequest.wLength);
    // ==== Standard Device Requests ====
    if ((UsbRequest.bmRequestType & (REQ_TYPE_MASK | REQ_RECIP_MASK)) == 0) {
//        CmdUnit.Printf("Standard Device Request\r");
        // Set Device Address
        if ((UsbRequest.bRequest == 0x05) && (UsbRequest.wIndex == 0) && (UsbRequest.wLength == 0)) {
            // Send zero data packet indicating Ok and wait for IN request to set received address after (checked in IN0Handler)
            Usb.Ep[0].SendZeroPacket();
            ControlStage = csWaitStatusIn;
        }
        // Get descriptor
        else if (UsbRequest.bRequest == 0x06) {
            uint8_t bValue = (uint8_t)(UsbRequest.wValue & 0x00FF);
            uint8_t bIndx = (uint8_t)(UsbRequest.wValue >> 8);
            switch (bValue) {
                case 1:     // Device descriptor
                    Usb.Ep[0].Buf = (uint8_t*)Virtual_Com_Port_DeviceDescriptor;
                    Usb.Ep[0].Count = 18;
                    Usb.Ep[0].WriteData();
                    ControlStage = csWaitDataIn;
                    break;
                case 2:     // Config descriptor
                    Usb.Ep[0].Buf = (uint8_t*)Virtual_Com_Port_ConfigDescriptor;
                    Usb.Ep[0].Count = (UsbRequest.wLength < CONFIG_DESCRIPTOR_SIZE)? UsbRequest.wLength : CONFIG_DESCRIPTOR_SIZE;
                    Usb.Ep[0].WriteData();
                    ControlStage = csWaitDataIn;
                    break;
                case 3:     // String descriptor
                    switch (bIndx) {
                        case 0:
                            Usb.Ep[0].Buf = (uint8_t*)Virtual_Com_Port_StringLangID;
                            Usb.Ep[0].Count = 4;
                            Usb.Ep[0].WriteData();
                            ControlStage = csWaitDataIn;
                            break;
                        case 1:
                            Usb.Ep[0].Buf = (uint8_t*)Virtual_Com_Port_StringVendor;
                            Usb.Ep[0].Count = 12;
                            Usb.Ep[0].WriteData();
                            ControlStage = csWaitDataIn;
                            break;
                        case 2:
                            Usb.Ep[0].Buf = (uint8_t*)Virtual_Com_Port_StringProduct;
                            Usb.Ep[0].Count = 32;
                            Usb.Ep[0].WriteData();
                            ControlStage = csWaitDataIn;
                            break;
                        case 3:
                            Usb.Ep[0].Buf = (uint8_t*)Virtual_Com_Port_StringSerial;
                            Usb.Ep[0].Count = 22;
                            Usb.Ep[0].WriteData();
                            ControlStage = csWaitDataIn;
                            break;
                        default: ControlStage = csWaitSetup; break;
                    }
                    break;
                default: ControlStage = csWaitSetup; break;
            }
        }
        // Set configuration
        else if ((UsbRequest.bRequest == 0x09) && (UsbRequest.wLength == 0)) {
            CmdUnit.Printf("SetConfiguration\r");
            // Perform all actions here needed to enable device
            Usb.IsConfigured = true;
            Usb.Ep[0].SendZeroPacket();
            ControlStage = csWaitStatusIn;
            TxLed.Blink(13); RxLed.Blink(13);
        }
        // all other is not supported
        else ControlStage = csWaitSetup;
    }

    // ==== Standard Interface Requests ====
    else if ((UsbRequest.bmRequestType & (REQ_TYPE_MASK | REQ_RECIP_MASK)) == 0x01) {
//        CmdUnit.Printf("Standard Interface Request\r");
        ControlStage = csWaitSetup; // Nothing is suported
    }

    // ==== Standard Endpoint Requests ====
    else if ((UsbRequest.bmRequestType & (REQ_TYPE_MASK | REQ_RECIP_MASK)) == 0x02) {
//        CmdUnit.Printf("Standard Endpoint Request\r");
        ControlStage = csWaitSetup; // Nothing is suported
    }

    // ==== Class Interface Requests ====
    else if ((UsbRequest.bmRequestType & (REQ_TYPE_MASK | REQ_RECIP_MASK)) == 0b00100001) {
//        CmdUnit.Printf("Class Interface Request\r");
        // ComPort specific requests
        if (UsbRequest.bRequest == 0x20) {
            ControlStage = csWaitDataOut;
        }
        else if (UsbRequest.bRequest == 0x21) {
            // Setup Com Configuration
//            uint8_t IBuf[7] = {0, 0xC2, 1, 0, 0, 0, 8};
            Usb.Ep[0].Buf = UartSettingsContanier;
            Usb.Ep[0].Count = 7;
            Usb.Ep[0].WriteData();
            ControlStage = csWaitDataIn;
//            CmdUnit.Printf("Sending sttings: %A\r", UartSettingsContanier, 7);
        }
        else if (UsbRequest.bRequest == 0x22) {
            Usb.Ep[0].Buf = 0;
            Usb.Ep[0].Count = UsbRequest.wLength;
            ControlStage = csWaitStatusIn;
        }
        else {
            CmdUnit.Printf("Else\r");
            ControlStage = csWaitSetup; // Nothing is suported
        }
    }

    // ==== All others ====
    else {
        CmdUnit.Printf("Something Else\r");
        ControlStage = csWaitSetup; // Nothing is suported
    }
}

void Post0Process(void) {
    Usb.Ep[0].SetRxCount(USB_MAX_PKT_SIZE);
    switch (ControlStage) {
        case csWaitSetup:
            Ep0RxState = epxStall;
            Ep0TxState = epxStall;
            break;
        case csWaitDataIn:
            Ep0RxState = epxNAK;
            Ep0TxState = epxValid;
            break;
        case csWaitDataOut:
            Ep0RxState = epxValid;
            Ep0TxState = epxNAK;
            break;
        case csWaitStatusIn:
            Ep0RxState = epxNAK;
            Ep0TxState = epxValid;
            break;
        case csWaitStatusOut:
            Ep0RxState = epxValid;
            Ep0TxState = epxStall;
            break;
        default: break;
    }
}

void In0Handler(void) {
    switch (ControlStage) {
        case csWaitDataIn:
            if (Usb.Ep[0].WriteData() == 0) ControlStage = csWaitStatusOut; // Otherwise stay in DataIn stage
            break;
        case csWaitStatusIn:
            // Check if set adress
            if ((UsbRequest.bmRequestType == 0) && (UsbRequest.bRequest == 0x05) && (UsbRequest.wIndex == 0) && (UsbRequest.wLength == 0)) {
                Usb.SetDeviceAddress((uint8_t)(UsbRequest.wValue >> 8));
                UsbRequest.bRequest = 0;    // Reset request
            }
            ControlStage = csWaitSetup;
            break;
        default:
            ControlStage = csWaitSetup;
            break;
    }
    Post0Process();
}

void Setup0Handler() {
//    CmdUnit.Printf("StpHNDL \r");
    union {
        uint8_t* b;
        uint16_t* w;
    } pBuf;

    switch (ControlStage) {
        case csWaitSetup:
            // Get USB request
            pBuf.b = PMAAddr + (uint8_t *)(Usb.Ep[0].GetRxAddr() * 2);    // *2 for 32 bits addr
            UsbRequest.bmRequestType = *pBuf.b++;               // bmRequestType
            UsbRequest.bRequest = *pBuf.b++;                    // bRequest
            pBuf.w++;                                           // word not accessed because of 32 bits addressing
            UsbRequest.wValue = __REV16(*pBuf.w++);             // wValue
            pBuf.w++;                                           // word not accessed because of 32 bits addressing
            UsbRequest.wIndex  = __REV16(*pBuf.w++);
            pBuf.w++;                                           // word not accessed because of 32 bits addressing
            UsbRequest.wLength = *pBuf.w;
            HandleCtrlRequest();
            break;
        default:
            ControlStage = csWaitSetup;
            break;
    }
    Post0Process();
}

void Out0Handler() {
    switch (ControlStage) {
        case csWaitStatusOut:
            ControlStage = csWaitSetup;
            break;
        case csWaitDataOut:
            Usb.Ep[0].Buf = UartSettingsContanier;  // No need in this information here;
            Usb.Ep[0].ReceivePkt();
            ControlStage = csWaitStatusIn;
            Device.Type = dtCOM_Port;
            Device.IsConfigured = false;
//            CmdUnit.Printf("Recieve sttings: %A\r", UartSettingsContanier, 7);
            break;
        default:;
            ControlStage = csWaitSetup;
            break;
    }
    Post0Process();
}
void Usb_t::ErrorPkt() {
    *ISTR = CLR_ERR;
    CmdUnit.Printf("ERR\r");
}

void Usb_t::NewPacketHandler() {                // Asking when usb interrupt enable and await for
    uint16_t PFlags = (uint16_t) *ISTR;
//    CmdUnit.Printf("IRQ flags: %X\r", PFlags);
    if(PFlags & ISTR_CTR)      HandlePkt();        // Correct Transfer
//    if(PFlags & ISTR_ERR)      ErrorPkt();         // Error ocured
//    if(PFlags & ISTR_WKUP)     WakeUp();           // Wake up
//    if(PFlags & ISTR_SUSP)     Suspend();          // Suspend
    if(PFlags & ISTR_RESET)    Reset();            // Reset
    if(PFlags & ISTR_SOF);                 // Start of frame
    if(PFlags & ISTR_ESOF);                 // Expected start of frame
}


// ================================ Interrupt ==================================
void USB_LP_CAN1_RX0_IRQHandler(void) {
//    Usb.IRQHandler();
    Usb.NewPacketHandler();
}
